/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Path Planner
 */

#include "pathPlanner.h"



void setParams()
{
    ROS_INFO("Setting parameters to server");

    ros::NodeHandle nh;
    //Coordinates of baskets
    nh.setParam("basket1_x",                    0.15); //0.15
    nh.setParam("basket1_y",                    -0.55);//-0.75
    nh.setParam("basket1_angle",                -1.5); // -1.5708 //90 degrees
    nh.setParam("basket2_x",                    2.55); //2.8
    nh.setParam("basket2_y",                    -0.6); //-0.85
    nh.setParam("basket2_angle",                0);

    nh.setParam("toy_safety_tolerance",         0.60); //0.6 //Distance in meters between robot's center and a toy after driving to the toy

    nh.setParam("max_wait_kinect",              8); //10 //Secs to wait Kinect observations
}




int main(int argc, char **argv)
{
    ros::init(argc, argv, "pathPlanner");

    ros::NodeHandle nodeHandle;
    PathPlanner pl(nodeHandle);

    //Subscribers
    ros::Subscriber subObjectList = nodeHandle.subscribe("/object_list", 50, &PathPlanner::updateObjectList, &pl);


    setParams();

    ros::Rate loop_rate(100);

    bool everythingOK = true;

	while(ros::ok())
	{
		ros::spinOnce();
		loop_rate.sleep();		
	}
	return 0;
}

PathPlanner::PathPlanner(ros::NodeHandle& nh) : mMoveBaseClient("move_base", true)
{
    mNodeHandle = nh;
    mNextGoal.target_pose.header.frame_id = "map";
    mCurrentGoalIdx = 0;
    mGoalTolerance = 0.2; // 20 cm
    mTimesInSamePosition = 0;
    mLastPositionAfterMovement = std::vector<double>(3, 0.0); //init starting pos
    mTimesTriedToSendSameGoal = 0;
    mToyCounter = 0;
    mCurrentToyBasket = 1;
    mToyToRemove.z = 0.15;

    mTriggerDetectPub = mNodeHandle.advertise<std_msgs::Bool>("triggerDetect", 10);
    mDeleteToyPub = mNodeHandle.advertise<geometry_msgs::Vector3>("/delete_objects", 10);

    mGoToGoalService = mNodeHandle.advertiseService("goToNextGoal", &PathPlanner::nextGoalService, this);

    setSearchingPaths();

    signal(SIGINT, PathPlanner::signalHandler);

    while(! mMoveBaseClient.waitForServer(ros::Duration(5.0)))
    {
        ROS_INFO("Waiting for the move_base action server to come up");
    }

    //setGoals();
}

/**
 * @brief PathPlanner::setSearchingPaths
 *
 * set hard-coded searching paths for area 1 and area 2
 */
void PathPlanner::setSearchingPaths()
{
    //Goalpoints for random path (x,y,angle)

    //AREA 1, close to basket 1
    std::vector<RandomPathGoal> goals1;

    RandomPathGoal goal1_1(0,0.0,-1.0); //-1.3
    goals1.push_back(goal1_1);

    RandomPathGoal goal1_1_help(0.2,-0.3,-1.0); //-1.3
    goals1.push_back(goal1_1_help);

    //RandomPathGoal goal1_2(0.2,-0.15,-0.1);
    RandomPathGoal goal1_2(0.2,-0.30,-0.1);
    goals1.push_back(goal1_2);

    //RandomPathGoal goal1_3(0.2,-0.15,0.7);
    RandomPathGoal goal1_3(0.2,-0.30,-2.1);
    goals1.push_back(goal1_3);

    //RandomPathGoal goal1_4(0.2,0.25,1.8);
    RandomPathGoal goal1_4(0.2,-0.3,1.5);
    goals1.push_back(goal1_4);

    //RandomPathGoal goal1_5(0.1,1.05,0.3);
    RandomPathGoal goal1_5(0.2,0.1,2);
    goals1.push_back(goal1_5);

    //RandomPathGoal goal1_6(0.1,1.05,-1.7);
    RandomPathGoal goal1_6(0.3,0.0,1.1);
    goals1.push_back(goal1_6);

    //RandomPathGoal goal1_7(0.1,-0.15,-2.1);
    RandomPathGoal goal1_7(0.2,0.5,1.5);
    goals1.push_back(goal1_7);

    RandomPathGoal goal1_8_added(0.3,0.8,2.1); //2.7
    goals1.push_back(goal1_8_added);

    RandomPathGoal goal1_8_added2(0.2,0.35,-2.6);
    goals1.push_back(goal1_8_added2);

    RandomPathGoal goal1_8_help(0.2,0.35,0);
    goals1.push_back(goal1_8_help);

   // RandomPathGoal goal1_8(0.9,0.35,0);
   // goals1.push_back(goal1_8);


    //AREA 2, close to basket 2
    std::vector<RandomPathGoal> goals2;

    RandomPathGoal goal2_1(1.65,0.35,-0.5); //1.55
    goals2.push_back(goal2_1);

    RandomPathGoal goal2_2(2.2,-0.15,-0.5);
    goals2.push_back(goal2_2);

    RandomPathGoal goal2_3(2.2,-0.15,0.6);
    goals2.push_back(goal2_3);

    //RandomPathGoal goal2_4(2.2,-0.15,-3.1);
    RandomPathGoal goal2_4(2.4,0,-2.7);
    goals2.push_back(goal2_4);

    RandomPathGoal goal2_4_added(2.4,0.2,1.5);
    goals2.push_back(goal2_4_added);

    //RandomPathGoal goal2_5(2.2,0.35,2.1);
    RandomPathGoal goal2_5(2.4,0.2,2.3);
    goals2.push_back(goal2_5);

    //RandomPathGoal goal2_6(2.1,0.65,1.35);
    RandomPathGoal goal2_6(2.0,0.5,0.8);
    goals2.push_back(goal2_6);

    //RandomPathGoal goal2_7(2.5,0.75,0.7);
    RandomPathGoal goal2_7(2.5,1,0.2);
    goals2.push_back(goal2_7);

    RandomPathGoal goal2_8(2.2,-0.45, -1.3);
    goals2.push_back(goal2_8);

    RandomPathGoal goal2_9(2.2,-0.45, -0.2);
    goals2.push_back(goal2_9);

    RandomPathGoal goal2_10(2.5,-0.75, -3);
    goals2.push_back(goal2_10);

    RandomPathGoal goal2_11_added(2.6,1.25,2.8);
    goals2.push_back(goal2_11_added);

    RandomPathGoal goal2_11(2.1,1.4, 2.6); //1.9 1.25
    goals2.push_back(goal2_11);

    RandomPathGoal goal2_door(1.7,1.45, 3); //1.9 1.25
    goals2.push_back(goal2_door);

    RandomPathGoalsInArea area1(goals1, 1);
    mArea1Path = area1;

    //for debugging just area 2
   // mArea1Path.travelled = true;

    RandomPathGoalsInArea area2(goals2, 2);
    mArea2Path = area2;
}

bool PathPlanner::nextGoalService(group6::goToNextGoal::Request &req, group6::goToNextGoal::Response &res)
{
    ROS_INFO("nextGoalService %d", req.question);
    bool failed = false;

    // Check if we have any toys left
    if(req.question == TOY && mToys.empty())
    {
        res.result = NO_TOYS_LEFT;
        return true;
    } 

    if(req.question == RANDOM_SEARCH)
    {
        mCurrentDestination = RANDOM_SEARCH;
        if(randomSearch())
        {
            res.result = SUCCEED;
        }
        else
        {
            res.result = NOT_SUCCEED;
        }
        return true;
    }

    if(!setNextGoal(req.question) || !proceedToNextGoal())
    {
        failed = true;
    }
    /*
    else
    {
        if(!proceedToNextGoal())
        {
            failed = true;
        }
    }
    */

    if(failed)
    {
        res.result = NOT_SUCCEED;
    }
    else
    {
        res.result = SUCCEED;
	/*
        if(mCurrentDestination == TOY)
        {
            removeCurrentToy();// NOT SURE WHEN TO REMOVE THIS FROM THE LIST, MAYBE AFTER MANIPULATION? /jerry
        }
	*/
    }

     if(mCurrentDestination == TOY)
        { 
            removeCurrentToy();// NOT SURE WHEN TO REMOVE THIS FROM THE LIST, MAYBE AFTER MANIPULATION? /jerry
        } 

    return true;
}

void PathPlanner::updateObjectList(const group6::DetectedObjectArray::ConstPtr &msg)
{

    /*
    if(mCurrentDestination != RANDOM_SEARCH)
    {
        return; // Skip messages that do not come during RANDOM_SEARCH
    }
    */


    mToys.clear();

    for(int i = 0; i < msg.get()->objects.size(); i++)
    {

        double x = msg.get()->objects[i].x;
        double y = msg.get()->objects[i].y;
        std::string id = msg.get()->objects[i].object_id;
        int area = chooseArea(x);


        ToyGoal toyGoal(x,y,area,id);
        mToys.push_back(toyGoal);
    }
    mRandomSearchInterrupted = true; //This will stop searching

    //debug
    printToys();
}

void PathPlanner::printToys()
{
    if(mToys.size() == 0)
    {
        ROS_INFO("NO TOYS!");
        return;
    }

    ROS_INFO("Current Toys:");
    for(int i = 0; i < mToys.size(); i++)
    {
        std::cout << mToys[i].x <<","<<mToys[i].y <<"  id:"<< mToys[i].id<< "   area" << mToys[i].area <<std::endl;
    }
}

void PathPlanner::removeCurrentToy()
{
    mToys.erase(mToys.begin()+mClosestToyIdx);
    mToyCounter++;

    mDeleteToyPub.publish(mToyToRemove);
}

int PathPlanner::chooseArea(double x)
{
    if(x > 1.3)// TODO: Have to check this limit in roboroom
    {
        return 2;
    }
    return 1;
}

bool PathPlanner::goToNextGoal(const double &xDirection, const double &yDirection, const double &angle)
{
    mNextGoal.target_pose.header.stamp = ros::Time(0);//ros::Time::now();
    mNextGoal.target_pose.pose.position.x = xDirection;
    mNextGoal.target_pose.pose.position.y = yDirection;
    mNextGoal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(angle);

    ROS_INFO("Sending new goal: %f, %f", xDirection, yDirection);
    mMoveBaseClient.sendGoal(mNextGoal);


    if(mCurrentDestination == RANDOM_SEARCH)
    {
        actionlib::SimpleClientGoalState currentState = mMoveBaseClient.getState();
        ros::Rate loopRate(25);

        int loopCount = 0;
        while(ros::ok() && !currentState.isDone())
        {
            loopCount++;
            loopRate.sleep();
            ros::spinOnce();

            //Cancel due to timeout
            if(loopCount > 30*25)
            {
                ROS_INFO("PathPlanner TIMEOUT during random search");
                mMoveBaseClient.cancelAllGoals();
                return false;
            }

            if(mRandomSearchInterrupted)
            {
                ROS_INFO("Goal cancelled");
                mMoveBaseClient.cancelAllGoals();
                return true;
            }
            currentState = mMoveBaseClient.getState();
        }
    }
    else
    {
        if(!mMoveBaseClient.waitForResult(ros::Duration(30.0)))
        {
            ROS_INFO("PathPlanner TIMEOUT!");
            return false;
        }
    
    }

    if(mMoveBaseClient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_INFO("Job done! Goal achieved!");
        return true;
    }else{
        ROS_INFO("FAILED, Goal not achieved!");
        return false;
    }
}


bool PathPlanner::proceedToNextGoal()
{

    if(mCurrentDestination == RANDOM_SEARCH)
    {
        return goToNextGoal(mCurrentGoal.first,mCurrentGoal.second,mNextAngle);
    }

    double angle;

    //Determine the angle for the goal
    if(mCurrentDestination == TOY)
    {
        tf::StampedTransform transform;
        if(!getCurrentTransform(transform))
        {
            return false;
        }
        double currentX = transform.getOrigin().x();
        double currentY = transform.getOrigin().y();

        angle = calculateAngleForNextGoal(currentX, currentY);

        //adjustToyGoal(transform); // changes goal coordinates so that the robot drives near the toy, not over the toy

        /* WE CAN USE THIS IF IT IS BETTER NOT TO MOVE IF WERE CLOSE TO THE TOY
         * IN THAT CASE THE UPPER adjustToyGoal(tranform) HAS TO BE COMMENTED
         */
        // Just need to turn, robot is so close to the toy
        if(checkIfCloseEnough(currentX, currentY))
        {
            ROS_INFO("Close to the toy, just need to turn!");
            mCurrentGoal.first = currentX;
            mCurrentGoal.second = currentY;
        }
        else // Have to move
        {
            adjustToyGoal(transform); // changes goal coordinates so that the robot drives near the toy, not over the toy
        }
    }
    else if(mCurrentDestination == BASKET_1)
    {
        mNodeHandle.getParam("basket1_angle", angle);
    }
    else if (mCurrentDestination == BASKET_2)
    {
        mNodeHandle.getParam("basket2_angle", angle);
    }
    else //INVALID CALL
    {
        return false;
    }


    ROS_INFO("PARAMS FOR THE NEXT GOAL: dx = %f, dy = %f , angle = %f", mCurrentGoal.first, mCurrentGoal.second, angle);

    //sending the goal
    return goToNextGoal(mCurrentGoal.first, mCurrentGoal.second, angle);
}


double PathPlanner::calculateAngleForNextGoal(const double& currentX, const double& currentY)
{
    double deltaX = mCurrentGoal.first- currentX;
    double deltaY = mCurrentGoal.second- currentY;

    return atan2(deltaY, deltaX);
}



bool PathPlanner::checkIfCloseEnough(const double &currentX, const double &currentY)
{
    double deltaX = mCurrentGoal.first - currentX;
    double deltaY = mCurrentGoal.second - currentY;
    double delta = sqrt(deltaX*deltaX + deltaY*deltaY);

    double tolerance;
    mNodeHandle.getParam("toy_safety_tolerance", tolerance);

    return delta < tolerance;
}

bool PathPlanner::checkIfStuckedInPlace(const double &currentX, const double &currentY,const double &currentZ)
{
    if(mCurrentGoalIdx == 0)
    {
        return false;
    }

    if (fabs(currentX-mLastPositionAfterMovement[0]) < 0.01 &&
        fabs(currentY-mLastPositionAfterMovement[1]) < 0.01 &&
        fabs(currentZ-mLastPositionAfterMovement[2]) < 0.01)
    {
        mTimesInSamePosition++; // Have not moved since last check
    }else{
        mTimesInSamePosition = 0;
        mLastPositionAfterMovement[0] = currentX;
        mLastPositionAfterMovement[1] = currentY;
        mLastPositionAfterMovement[2] = currentZ;
    }

    if(mTimesInSamePosition > 50)
    {
        ROS_INFO("Robot has been in the same place for more than 5sec!");
        mTimesInSamePosition = 0;
        return true; // Have been in same place for 50 time steps == 5 sec
    }
    return false; // everything OK
}

void PathPlanner::addHelperGoal(const double &currentX, const double &currentY)
{
    //Helper should be in the middle of current position and next real goal point
    double helperGoalX = currentX + (mGoals[mCurrentGoalIdx].first - currentX)/2;
    double helperGoalY = currentY + (mGoals[mCurrentGoalIdx].second - currentY)/2;

    double distanceToHelper = (helperGoalX - currentX)*(helperGoalX - currentX) +
			      (helperGoalY - currentY)*(helperGoalY - currentY);

    if(distanceToHelper < 0.09) // DISTANCE TO GOAL <0.3m
    {
	return;
    }	

    //insert new helper goal to mGoals
    std::vector<std::pair<double,double> >::iterator iter = mGoals.begin()+ mCurrentGoalIdx;
    mGoals.insert(iter, std::make_pair(helperGoalX, helperGoalY));
    mTotalGoals++;
    mTimesTriedToSendSameGoal = 0;

    ROS_INFO("Added new helper goal: %f %f", helperGoalX, helperGoalY);
}

bool PathPlanner::getCurrentTransform(tf::StampedTransform &currentTransform)
{
    try
    {
        ros::Time now = ros::Time(0);//ros::Time::now();
        mListener.waitForTransform("/map", "/base_link",now, ros::Duration(1));//ros::Time(0)
        mListener.lookupTransform("/map","/base_link",now, currentTransform); //ros::Time(0)
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("%s",ex.what());
        return false;
    }
    return true;
}

bool PathPlanner::setNextGoal(int dest)
{
    ROS_INFO("Setting next goal");
    if(dest == BASKET_1 || dest == BASKET_2)
    {
        // THE BASKET IS DECIDED HERE. NO NEED FOR STATE MACHINE TO DECIDE
        if(mCurrentToyBasket ==1)
        {
            mNodeHandle.getParam("basket1_x", mCurrentGoal.first);
            mNodeHandle.getParam("basket1_y", mCurrentGoal.second);
            mCurrentDestination = BASKET_1;
        }
        else
        {
            mNodeHandle.getParam("basket2_x", mCurrentGoal.first);
            mNodeHandle.getParam("basket2_y", mCurrentGoal.second);
            mCurrentDestination = BASKET_2;
        }
    }
    else if(dest == TOY)
    {
        mCurrentDestination = TOY;
        if(!setNearestToyGoal())
        {
            return false;
        }
    }
    else // INVALID CALL
    {
        return false;
    }

    return true;
}

bool PathPlanner::randomSearch()
{
    mRandomSearchInterrupted = false;
    int frequency = 10;
    ros::Rate loopRate(frequency);
    double loopCount = 0;
    double maxWait;

    std_msgs::Bool boolToDetection;
    boolToDetection.data = true;
    //mTriggerKinectClient.publish(boolToKinect);

    if(!mNodeHandle.getParam("max_wait_kinect", maxWait))
    {
        ROS_ERROR("PARAM SERVER ERROR IN PATHPLANNER randomSearch()");
        return false;
    }

    while(!mRandomSearchInterrupted)
    {
        setRandomPathGoal();
        ROS_INFO("Random search next goal: %f %f %f",mCurrentGoal.first, mCurrentGoal.second, mNextAngle);
        if(!proceedToNextGoal())
        {
            return false;
        }

        if(!mRandomSearchInterrupted)
        {
            updatePath();
        }

        //Start detection
        boolToDetection.data = true;
        mTriggerDetectPub.publish(boolToDetection);

        //Wait for kinect observations
        loopCount = 0;
        while(loopCount <= maxWait*frequency)
        {
            if(mRandomSearchInterrupted)
            {
                break;
            }
            ros::spinOnce();
            loopRate.sleep();
            loopCount++;
        }

        //Stop detection
        boolToDetection.data = false;
        mTriggerDetectPub.publish(boolToDetection);

    }
    return true;
}

void PathPlanner::setRandomPathGoal()
{
    if(checkWhichAreaToSearch() == 1)
    {
        mCurrentGoal.first = mArea1Path.goalsInArea[mArea1Path.currentIdx].x;
        mCurrentGoal.second = mArea1Path.goalsInArea[mArea1Path.currentIdx].y;
        mNextAngle = mArea1Path.goalsInArea[mArea1Path.currentIdx].z;
    }
    else
    {
        mCurrentGoal.first = mArea2Path.goalsInArea[mArea2Path.currentIdx].x;
        mCurrentGoal.second = mArea2Path.goalsInArea[mArea2Path.currentIdx].y;
        mNextAngle = mArea2Path.goalsInArea[mArea2Path.currentIdx].z;
    }
}

int PathPlanner::checkWhichAreaToSearch()
{
    if(mArea1Path.travelled)
    {
        if(mArea2Path.travelled) //Both travelled
        {
            mArea1Path.travelled = false;
            mArea1Path.currentIdx = 0;
            mArea2Path.travelled = false;
            mArea2Path.currentIdx = 0;
            return 1;
        }
        else // Only Area 1 travelled
        {
            return 2;
        }
    }
    else // Area 1 not travelled yet
    {
        return 1;
    }
}

void PathPlanner::updatePath()
{
    if(!mArea1Path.travelled)
    {
        mArea1Path.currentIdx++;
        if(mArea1Path.currentIdx == mArea1Path.pathSize)
        {
            mArea1Path.travelled = true;
        }
    }
    else
    {
        mArea2Path.currentIdx++;
        if(mArea2Path.currentIdx == mArea2Path.pathSize)
        {
            mArea2Path.travelled = true;
        }
    }
}


void PathPlanner::signalHandler(int signal)
{
        ROS_INFO("CAUGHT SIGNAL: %d", signal);
        if(signal == 2)
        {
            exit(1);
        }
}

bool PathPlanner::setNearestToyGoal()
{
    ROS_INFO("setNearestToyGoal");
    if(mToys.empty())
    {
        return false;
    }

    tf::StampedTransform currentTransform;
    if(!getCurrentTransform(currentTransform))
    {
        return false;
    }

    int currentArea = chooseArea(currentTransform.getOrigin().x());


    double closestDistance = 10000;
    double distanceToToy = 0;
    bool found = false;
    bool differentArea = false;

    //First check toys from current Area
    for(int i = 0; i < mToys.size(); i++)
    {
        if(mToys[i].area != currentArea)
        {
            continue;
        }
        found = true;
        distanceToToy = getDistanceToToy(currentTransform, i);
        if(closestDistance > distanceToToy)
        {
            mClosestToyIdx = i;
            closestDistance = distanceToToy;
        }
    }

    //Check from other area if not found yet
    if(!found)
    {
        differentArea = true;
        for(int i = 0; i < mToys.size(); i++)
        {
            if(mToys[i].area == currentArea)
            {
                continue;
            }
            distanceToToy = getDistanceToToy(currentTransform, i);
            if(closestDistance > distanceToToy)
            {
                mClosestToyIdx = i;
                closestDistance = distanceToToy;
            }
        }
    }

    ROS_INFO("Toy in different area? %d", differentArea);

    mCurrentGoal.first =mToys[mClosestToyIdx].x; //mToys[mClosestToyIdx].first.first;
    mCurrentGoal.second = mToys[mClosestToyIdx].y;//mToys[mClosestToyIdx].first.second;
    setToyBasket();

    //Check if we have to go through helper before approaching toy
    enum specialCase helper= checkForSpecialCases(currentTransform);
    if(helper != NONE)
    {
        if(!goToHelper(helper))
        {
            ROS_ERROR("FAILED TO REACH HELPER GOAL");
            return false;
        }

        //Have to get new currentTransform because the robot has moved
        if(!getCurrentTransform(currentTransform))
        {
            return false;
        }
    }


    ROS_INFO("Toy: %f, %f", mCurrentGoal.first, mCurrentGoal.second);
    mToyToRemove.x = mCurrentGoal.first;
    mToyToRemove.y = mCurrentGoal.second;

    //adjustToyGoal(currentTransform); // changes goal coordinates so that the robot drives near the toy, not over the toy

    ROS_INFO("Toy adjusted GOAL: %f, %f", mCurrentGoal.first, mCurrentGoal.second);
    return true;
}

void PathPlanner::setToyBasket()
{
    int toyArea = chooseArea(mCurrentGoal.first);
    if(toyArea == 2 && mToys[mClosestToyIdx].id.find("big") != std::string::npos)
    {
        mCurrentToyBasket = 2;
    }
    else
    {
        mCurrentToyBasket = 1;
    }
}

//Function for checking if we have to go through helper goal before approaching the toy
PathPlanner::specialCase PathPlanner::checkForSpecialCases(const tf::StampedTransform &currentTransform)
{

    int robotArea = chooseArea(currentTransform.getOrigin().x());
    int toyArea = chooseArea(mCurrentGoal.first);

    if(mCurrentGoal.second > 0.9 &&
       (mCurrentGoal.first > 0.8   && mCurrentGoal.first < 1.4))
    {
        if(robotArea == 1 &&  currentTransform.getOrigin().y() < 1.02)
        {
            return BEHIND_BOX_1;
        }
        else if(robotArea == 2 && currentTransform.getOrigin().y() < 1.15)
        {
            return BEHIND_BOX_2;
        }
    }
    else if (robotArea == 1 && toyArea == 2 && mCurrentGoal.first < 1.6 && mCurrentGoal.second > -0.05 && mCurrentGoal.second < 0.78)
    {
        return NONE;
    }
    else if(mCurrentGoal.first > 2.9 &&
            (mCurrentGoal.second < 0.5 && mCurrentGoal.second > 0))
    {
        return AREA2_BEHIND_BAR_RIGHT;
    }
    else if(mCurrentGoal.first > 2.9 &&
            (mCurrentGoal.second < 1.1 && mCurrentGoal.second > 0.6))
    {
        return AREA2_BEHIND_BAR_LEFT;
    }
    else if(mCurrentGoal.second < -0.9 &&
            (mCurrentGoal.first > 2.9 || (mCurrentGoal.first > 1.35 && mCurrentGoal.first < 2)))
    {
        return AREA2_CLOSE_TO_WALL;
    }
/*
    else if(robotArea == 1 && toyArea == 2 &&
            (mCurrentGoal.first < 2.3 && mCurrentGoal.second > -0.05 && mCurrentGoal.second < 0.85))
    {
        return AREA1_TO_AREA2_DOOR;       
    }
*/ 
    else if(robotArea == 1 && toyArea == 2) //Going from area 1 to area 2
    {
        return AREA1_TO_AREA2;
    }
    else if(robotArea == 2 && toyArea == 1)
    {
        return AREA2_TO_AREA1;
    }
    else // No need for helper goal
    {
        return NONE;
    }
    return NONE;
}

bool PathPlanner::goToHelper(specialCase helper)
{
    double x = 0;
    double y = 0;
    double angle = 0;


    //NONE = 0, BEHIND_BOX_1 = 1, BEHIND_BOX_2 = 2, AREA2_CLOSE_TO_WALL = 3, AREA2_BEHIND_BAR_RIGHT = 4, AREA2_BEHIND_BAR_LEFT = 5, AREA1_TO_AREA2 = 6, AREA2_TO_AREA1 = 7}
    switch (helper) {
    case AREA1_TO_AREA2:
    {
        ROS_INFO("Going from area 1 to 2 helper");
        x = 2.0; //1.7
        y = 0.3;
        angle = 0;
        break;
    }
    case AREA2_TO_AREA1:
    {
        ROS_INFO("Going from area 2 to 1 helper");
        x = 0.3;
        y = -0.2;
        angle = 1.5;
        break;
    }
    case BEHIND_BOX_1:
    {
        ROS_INFO("Going to area 1 behind box helper");
        x = 0.1; //-0.1
        y = 1.15; //1.05
        angle = 0.2;
        break;
    }
    case BEHIND_BOX_2:
    {
        ROS_INFO("Going to area 2 behind box helper");
        x = 2.6; //2.5 
        y = 1.25; //1.1
        angle = 2.7;
        break;
    }
    case AREA2_CLOSE_TO_WALL:
    {
        ROS_INFO("Going to area 2 close to wall helper");
        x = 2.3;
        y = -0.7;
        angle = -1.5;
        break;
    }
    case AREA2_BEHIND_BAR_RIGHT:
    {
        ROS_INFO("Going to area 2 behind bar helper RIGHT");
        x = 2.2;
        y = -0.15;
        angle = 0.6;
        break;
    }
    case AREA2_BEHIND_BAR_LEFT:
    {
        ROS_INFO("Going to area 2 behind bar helper LEFT");
        x = 2.5;
        y = 1.2;
        angle = -0.5;
        break;
    }
        case AREA1_TO_AREA2_DOOR:
    {
        ROS_INFO("Going to helper before door between area 1 and area 2");
        x = 0.6;
        y = 0.35;
        angle = 0;
        break;
    }

    default:
    {
        ROS_ERROR("INVALID HELPER GOAL CASE!");
        return true;
        break;
    }
    }

    mNextGoal.target_pose.header.stamp = ros::Time(0);
    mNextGoal.target_pose.pose.position.x = x;
    mNextGoal.target_pose.pose.position.y = y;
    mNextGoal.target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(angle);

    ROS_INFO("Going to helper goal: %f, %f, %f", x, y, angle);
    mMoveBaseClient.sendGoal(mNextGoal);

    if(!mMoveBaseClient.waitForResult(ros::Duration(30)))
    {
        ROS_INFO("helper goal TIMEOUT!");
        mMoveBaseClient.cancelAllGoals();
        return false;
    }

    if(mMoveBaseClient.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
    {
        ROS_ERROR("Failed to reach helper goal");
        return false;
    }

    ROS_INFO("Helper goal done");
    return true;
}


int PathPlanner::checkHelperGoal(int area)
{
    if(area == 2)
    {
        if(mToys[mClosestToyIdx].y < 0)
        {
            return 1;
        }
        else
        {
            return 2;
        }
    }

}

double PathPlanner::getDistanceToToy(const tf::StampedTransform &currentTransform, const int &toyIdx)
{    
    double deltaX = mToys[toyIdx].x - currentTransform.getOrigin().x();//mToys[toyIdx].first.first - currentTransform.getOrigin().x();
    double deltaY = mToys[toyIdx].y - currentTransform.getOrigin().y();//mToys[toyIdx].first.second - currentTransform.getOrigin().y();

    double distance = sqrt(
                pow((deltaX), 2)+
                pow((deltaY), 2));

    ROS_INFO("distance for idx %d = %f", toyIdx, distance);

    return distance;
}


//Changes to goal coordinates so that the robot wont drive over the toy.
//The robot drives to a point that is "toy_safety_tolerance" from the toy
void PathPlanner::adjustToyGoal(const tf::StampedTransform &currentTransform)
{
    /*
    // Dummy implementation for mid term 2
    double safetyDistance;
    mNodeHandle.getParam("toy_safety_tolerance", safetyDistance);
    
    mCurrentGoal.first -= safetyDistance;
    */

    double currentX = currentTransform.getOrigin().x();
    double currentY = currentTransform.getOrigin().y();
    ROS_INFO("Current X %f  Current Y %f", currentX, currentY);

    double deltaXOriginal = mCurrentGoal.first - currentX;
    double deltaYOriginal = mCurrentGoal.second- currentY;
    double angle = atan2(deltaXOriginal,deltaYOriginal);

    double safetyDistance;
    mNodeHandle.getParam("toy_safety_tolerance", safetyDistance);

    double deltaX = fabs(sin(angle)*safetyDistance);
    double deltaY = fabs(cos(angle)*safetyDistance);

    //ADJUST X
    if (currentX < mCurrentGoal.first)
    {
        mCurrentGoal.first -= deltaX;
    }
    else
    {
         mCurrentGoal.first += deltaX;
    }


    //ADJUST Y
    if(currentY < mCurrentGoal.second)
    {
        mCurrentGoal.second -= deltaY;
    }
    else
    {
        mCurrentGoal.second += deltaY;
    }
}



